#include "CollisionThread.h"                    


xQueueHandle CollisionThreadQueue;

extern xQueueHandle CommandThreadQueue;
extern xQueueHandle DataManagerThreadQueue;
extern xQueueHandle WebQueue;   
extern xQueueHandle UARTQueue;

Comms_Msg_Struct *Msg;
Comms_Msg_Struct cmdOut;
Comms_Msg_Struct webOut;
Comms_Msg_Struct *cmdOutMsg;
Comms_Msg_Struct *webOutMsg;

Collision_Data_Enum CollisionData;

unsigned int LIR_45;
unsigned int LIR_90;

unsigned int RIR_90;
unsigned int RIR_45;

const unsigned int LIR_45_TH = 10;
const unsigned int LIR_90_TH = 11;

const unsigned int RIR_90_TH = 11;
const unsigned int RIR_45_TH = 10;

int RIR_90_Prev, RIR_45_Prev, count;

Collision_Data_Enum state;

void CollisionThreadTaskCode( void * pvParameters){
        
        cmdOut.msgType = Collision;
        cmdOut.size = 1;
        cmdOut.counter = 0;
        int i;
        for(i=0; i<6; i++)
                cmdOut.data[i] = 0;
        cmdOutMsg = &cmdOut;
        
        webOut.msgType = Collision;
        webOut.size = 3;
        webOut.counter = 0;
        //int i;
        for(i=0; i<6; i++)
                webOut.data[i] = 0;
        webOutMsg = &webOut;

        if(WebQueue !=0)
                if(xQueueSend( WebQueue, (void * ) &webOutMsg, ( portTickType ) 0 ) != pdPASS){};

        state = Straight;
        count = 0;
        
        for(;;){
        
                if( CollisionThreadQueue != 0 )
                {
                        if( xQueueReceive( CollisionThreadQueue, (void*)&( Msg ), ( portTickType ) 0 ) ) 
                        { 
                                // check if from data manager
                                
                                LIR_45 = Msg->data[0];
                                LIR_90 = Msg->data[1];
                                RIR_90 = Msg->data[2];
                                RIR_45 = Msg->data[3];

//                              static int RIR_90_Prev = RIR_90;
//                              static int RIR_45_Prev = RIR_45;
//
//                              if(count==1)
//                              {
//                                      if((RIR_90_Prev - RIR_90)> 180)
//                                      RIR_90 += 255;
//                              
//                                      if((RIR_45_Prev - RIR_45)> 180)
//                                      RIR_45 += 255;
//                                      count = 0;
//                              }

                                switch(state){
                                        case Straight:
                                                state = Control_Straight();
                                        break;
                                        case SlightLeft:
                                                state = Control_SlightLeft();
                                        break;
                                        case SlightRight:
                                                state = Control_SlightRight();
                                        break;
                                        case TurnLeft:
                                                state = Control_TurnLeft();
                                        break;
                                        case TurnRight:
                                                state = Control_TurnRight();
                                        break;
                                        default:
                                                state = Stop;
                                        break;
                                }
                                //state = Control_Straight();
                                // save Prev Values
//                              RIR_90_Prev =  RIR_90;
//                              RIR_45_Prev     =  RIR_45;

                                cmdOut.msgType = Collision;
                                cmdOut.size = 2;
                                cmdOut.counter = 0;
                                cmdOut.data[0] = state;
                                count++;
                                //PreviousFrontIR = FrontInfra;
                                
                                
//                              switch(state){
//                                      case Straight:
//                                              cmdOut.data[0] = CMDStraightL; 
//                                              cmdOut.data[1] = CMDStraightR;
//                                      break;
//                                      case SlightLeft:
//                                              cmdOut.data[0] = CMDSlightLeftL; 
//                                              cmdOut.data[1] = CMDSlightLeftR;
//                                              
//                                      break;
//                                      case SlightRight:
//                                              cmdOut.data[0] = CMDSlightRightL; 
//                                              cmdOut.data[1] = CMDSlightRightR;
//                                              
//                                      break;
//                                      case TurnLeft:
//                                              cmdOut.data[0] = CMDTurnLeftL; 
//                                              cmdOut.data[1] = CMDTurnLeftR;
//                                      break;
//                                      case TurnRight:
//                                              cmdOut.data[0] = CMDTurnRightL; 
//                                              cmdOut.data[1] = CMDTurnRightR;
//                                      break;
//                                      default:
//                                              cmdOut.data[0] = CMDStopL; 
//                                              cmdOut.data[1] = CMDStopR;
//                                      break;
//                              }
                                
                        if(CommandThreadQueue !=0)
                                if(xQueueSend( CommandThreadQueue, (void * ) &cmdOutMsg, ( portTickType ) 0 ) != pdPASS){};
                                
                                        webOut.msgType = Collision;
                                        webOut.size = 4;
                                        webOut.counter = 0;
                                        webOut.data[0] = LIR_45;
                                        webOut.data[1] = LIR_90;
                                        webOut.data[2] = RIR_90;
                                        webOut.data[3] = RIR_45;
                                
                        if(WebQueue !=0)
                                if(xQueueSend( WebQueue, (void * ) &webOutMsg, ( portTickType ) 0 ) != pdPASS){};
                                        
                        }
                }       
        }
}

void Init_CollisionThread()
{
        xTaskHandle CollisionThreadHandle;
        //static unsigned char ucParameterToPass;       
        /* Create the task, storing the handle.  Note that the passed parameter 
        ucParameterToPass must exist for the lifetime of the task, so in this 
        case is declared static.  If it was just an an automatic stack variable 
        it might no longer exist, or at least have been corrupted, by the time
        the new task attempts to access it. */
        xTaskCreate( CollisionThreadTaskCode, ( signed char * ) "Collision Thread Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
        , &CollisionThreadHandle );
        CollisionThreadQueue = xQueueCreate( 100, sizeof(Comms_Msg_Struct *)  );
}


Collision_Data_Enum Control_Straight(){


                        if(LIR_45 < LIR_45_TH && LIR_45 < RIR_45){
                                return TurnRight;
                        }else if(RIR_45 < RIR_45_TH){
                                return TurnLeft;
						}
//                      }else if(LIR_0 < LIR_0_TH && LIR_0 < RIR_0){
//                                return TurnRight;
//                        }else if(RIR_0 < RIR_0_TH){
//                                return TurnLeft;
//                        }
						
                       else if(LIR_90 < LIR_90_TH && LIR_90 < RIR_90)
                               return TurnRight;
                       else if(RIR_90 < RIR_90_TH){
                               return TurnLeft;
                       }
					   else if( abs(LIR_90 - RIR_90) < 35 && LIR_90 < LIR_90_TH && RIR_90 < RIR_90_TH){
                               if( LIR_45 < RIR_45){
                                       return TurnRight;
                               }else if(RIR_45 < LIR_45){
                                       return TurnLeft;
                               }else{
                                       return TurnRight;
                               }
                       }
						else{
                                return Straight;
                        }
                        
                          // if(FrontInfra < FIR_TH){
                                // if(LeftInfra > RightInfra && LeftInfra >= LIR_TH ){
                                        // return TurnLeft;
                                // }else if(RightInfra > LeftInfra && RightInfra >= RIR_TH){
                                        // return TurnRight;
                                // }else if(RightUltraSensor > LeftUltraSensor && RightUltraSensor >= RUS_TH){
                                        // return TurnRight;
                                // }else if(LeftUltraSensor > RightUltraSensor && LeftUltraSensor >= LUS_TH){
                                        // return TurnLeft;
                                // }else{
                                        // return Straight;
                                // }
                          // }else{
                                        // return TurnRight;
                          // }



}

Collision_Data_Enum Control_SlightLeft(){

//      if(RightInfra > RIR_TH)
//              return Straight;        
//
//      return SlightLeft;

}

Collision_Data_Enum Control_SlightRight(){

        // if(LeftInfra > LIR_TH)
                // return Straight;
        // return SlightRight;
        
}

Collision_Data_Enum Control_TurnLeft(){
        if(RIR_45 > RIR_45_TH && RIR_90 > RIR_90_TH){
                return Straight;
        }else{
                return TurnLeft;
        } 




		
}

Collision_Data_Enum Control_TurnRight(){
        if(LIR_45 > LIR_45_TH && LIR_90 > LIR_90_TH){
                return Straight;
        }else{
                return TurnRight;
        }       
}
// unsigned int IsValid(unsigned int Sensor_value, unsigned int TH){
        // if(Sensor_value >= TH){
                // return 1; 
        // }
        // else
                // return 0;
// }